#include "encode.h"

/*
    右边电机
    M1A——PF0 PHA0    M1B——PF1 PHB0

    左边电机
    M2A——PG0 PHA1    M2B——PG1 PHB1
*/

extern motor_config myMotor_Para;   //无刷直流电机参数的配置结果

motor_config myMotor_Para={
	.wheel_radius_cm			=		tire_radius_cm_default,				//驱动轮的半径
	.pulse_num_per_circle		=		pulse_cnt_per_circle_default,			//驱动轮转动一圈时编码器的脉冲累计值
};

encoder getEncoder;       //编码器参数的获取
getTime timer_qei1,timer_qei0;

/**
 * @brief QEI0_IRQHandler
 * 
 * 编码器0的中断服务函数，通过速度定时器计时20ms溢出中断
 * 
 */
void QEI0_IRQHandler(void)
{
	getTime_Period(&timer_qei0);   							//获取两次进入中断的时间
    QEIIntClear(QEI0_BASE,QEI_INTTIMER);    				//清除编码器中断
    /*
      QEIVelocityGet这个函数通过访问编码器中的速度累加器的speed寄存器，返回周期的脉冲数
    在我们设定的速度定时器的装载值为为SysCtlClockGet()/50=80M/50=1.6M  也就是20ms进入一次编码器中断。
    所以就是以20ms为周期的频率进入中断读取脉冲数
    */
    getEncoder.right_motor_cnt=QEIVelocityGet(QEI0_BASE); 	        //获取单个周期的脉冲采样数 20ms
	getEncoder.right_motor_period_ms=timer_qei0.Time_Delta_INT;
    
    if(QEIDirectionGet(QEI0_BASE)==1)	    				//获取编码脉冲方向
        getEncoder.right_motor_dir=1.0f;
	else 
        getEncoder.right_motor_dir=-1.0f;
	
    getEncoder.right_motor_total_cnt+=getEncoder.right_motor_cnt*getEncoder.right_motor_dir;//计算总的脉冲数

    getEncoder.right_motor_location_cnt+=getEncoder.right_motor_cnt*getEncoder.right_motor_dir;
    if(getEncoder.right_motor_location_cnt>1060)getEncoder.right_motor_location_cnt=0;
    else if(getEncoder.right_motor_location_cnt<-1060)getEncoder.right_motor_location_cnt=0;
}

/**
 * @brief Get the Right Motor Speed object
 * 获取右边电机的旋转速度
 * @return float 
 */
float getRight_Motor_Speed(void)
{
	//获取轮胎每分钟转的圈数 圈/分
	/*
		getEncoder.right_motor_cnt 单个周期的脉冲采样数
		getEncoder.right_motor_dir 编码器产生的脉冲方向
		trackless_motor.pulse_num_per_circle 转一圈的理论脉冲数 1060
		getEncoder.right_motor_period_ms 采集脉冲所用的时间 /ms
	*/
	getEncoder.right_motor_speed_rpm=60*(getEncoder.right_motor_cnt*getEncoder.right_motor_dir/myMotor_Para.pulse_num_per_circle)
										/(getEncoder.right_motor_period_ms*0.001f);
	//获取轮胎的速度 厘米/秒
	/*
		trackless_motor.wheel_radius_cm 轮胎的半径 2.40f /cm
		getEncoder.right_motor_speed_rpm 轮胎每分钟转的圈数
	*/
	getEncoder.right_motor_speed_cmps=2*3.14f*myMotor_Para.wheel_radius_cm*(getEncoder.right_motor_speed_rpm/60);
	
	return getEncoder.right_motor_speed_cmps;		//返回轮胎的速度
}

/**
 * @brief QEI1_IRQHandler
 * 
 * 编码器1的中断服务函数，通过速度定时器计时20ms溢出中断
 * 
 */
void QEI1_IRQHandler(void)
{
	getTime_Period(&timer_qei1);   							//获取两次进入中断的时间
    QEIIntClear(QEI1_BASE,QEI_INTTIMER);    				//清除编码器中断
    getEncoder.left_motor_cnt=QEIVelocityGet(QEI1_BASE); 	//获取单个周期的脉冲采样数 20ms
	getEncoder.left_motor_period_ms=timer_qei1.Time_Delta_INT;  //获取的时间
    
    if(QEIDirectionGet(QEI1_BASE)==1)	    	    //获取编码脉冲方向
        getEncoder.left_motor_dir=-1.0f;
	else 
        getEncoder.left_motor_dir=1.0f;
	
    getEncoder.left_motor_total_cnt+=getEncoder.left_motor_cnt*getEncoder.left_motor_dir;//计算总的脉冲数

    getEncoder.left_motor_location_cnt+=getEncoder.left_motor_cnt*getEncoder.left_motor_dir;
    if(getEncoder.left_motor_location_cnt>1060)getEncoder.left_motor_location_cnt=0;
    else if(getEncoder.left_motor_location_cnt<-1060)getEncoder.left_motor_location_cnt=0;
}

/**
 * @brief Get the leftt Motor Speed object
 * 获取左边电机的旋转速度
 * @return float 
 */
float getLeft_Motor_Speed(void)
{
	//获取轮胎每分钟转的圈数 圈/分
	getEncoder.left_motor_speed_rpm=60*(getEncoder.left_motor_cnt*getEncoder.left_motor_dir/myMotor_Para.pulse_num_per_circle)
									/(getEncoder.left_motor_period_ms*0.001f);
	//获取轮胎的速度 厘米/秒
	getEncoder.left_motor_speed_cmps=2*3.14f*myMotor_Para.wheel_radius_cm*(getEncoder.left_motor_speed_rpm/60);
	
	return getEncoder.left_motor_speed_cmps;		//返回轮胎的速度
}

void ENCODE_Init(void)
{
	/*
		这段代码很重要
	  第一行代码：这行代码将GPIO端口F的锁定寄存器设置为一个特定的键值（GPIO_LOCK_KEY）。在许多微
	控制器中，GPIO寄存器配置前需要先进行锁定，以防止在配置过程中发生意外的写入。
	  第二行代码：这行代码将GPIO端口F的引脚配置寄存器（GPIO_O_CR）中的第0位设置为高电平。GPIO_O_CR
	通常用于配置GPIO引脚的功能，例如是用作通用输入/输出还是其他专用功能。通过使用位运算符|=，代码
	确保只修改第0位，而不影响其他位的配置。
	  第三行代码：这行代码将GPIO端口F的锁定寄存器重置为0，这通常表示解锁GPIO寄存器，允许进一步的配置更改。
	
		防止在配置过程中的意外写入。
	*/
	HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
	HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= GPIO_PIN_0;
	HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = 0;
	
    //右边电机编码器初始化
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI0);
    while(ROM_SysCtlPeripheralReady(SYSCTL_PERIPH_QEI0)!=true);
    //GPIO口复用为编码器A相输入
    GPIOPinConfigure(GPIO_PF0_PHA0);
    GPIOPinConfigure(GPIO_PF1_PHB0);
    //配置GPIO引脚为QEI外设
    GPIOPinTypeQEI(GPIO_PORTF_BASE,GPIO_PIN_0);
    GPIOPinTypeQEI(GPIO_PORTF_BASE,GPIO_PIN_1);
    //GPIO引脚配置为上拉
    GPIOPadConfigSet(GPIO_PORTF_BASE,GPIO_PIN_0,GPIO_STRENGTH_4MA,GPIO_PIN_TYPE_STD_WPU);
	GPIOPadConfigSet(GPIO_PORTF_BASE,GPIO_PIN_1,GPIO_STRENGTH_4MA,GPIO_PIN_TYPE_STD_WPU);
    /*
        配置了控制寄存器以及最大位置寄存器（积分器的最大值）
    */
    QEIConfigure(QEI0_BASE,(QEI_CONFIG_CAPTURE_A_B      //使用AB相双边沿计数
                |QEI_CONFIG_NO_RESET	   //索引脉冲不复位位置积分器。
                |QEI_CONFIG_QUADRATURE      //在ChA和ChB上提供正交信号
                |QEI_CONFIG_NO_SWAP),       //交换ChA和ChB的通道
                0);
    //配置编码器的捕获速度
    /*
        第二个参数为指定在计数之前应用于输入正交信号的预分频
        第三个参数为指定测量速度的时钟滴答数；必须非零
        配置了速度定时器的预分频器（在控制寄存器中）
        配置了速度定时器的重装载寄存器，装载值
    */
    QEIVelocityConfigure(QEI0_BASE,QEI_VELDIV_1,SysCtlClockGet()/50);
	QEIVelocityEnable(QEI0_BASE);
    QEIEnable(QEI0_BASE);       //使能正交编码器
    QEIIntRegister(QEI0_BASE,QEI0_IRQHandler);
    /*
        当速度定时器计时达到速度定时器设定的装载值就触发中断，速度定时器的装载值设定
    为SysCtlClockGet()/50=80M/50=1.6M   也就是20ms进入一次编码器中断。
        时间计算为1/80M×1.6M=20ms
    */
    QEIIntEnable(QEI0_BASE,QEI_INTTIMER);//函数启用指定的正交编码器中断源。中断将被发送到中断控制器
    IntEnable(INT_QEI0);        //使能中断
	
	/************************************************************************************************************/
    //左边电机编码器初始化
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI1);
    while(ROM_SysCtlPeripheralReady(SYSCTL_PERIPH_QEI1)!=true);
    //GPIO口复用为编码器A相输入
    GPIOPinConfigure(GPIO_PG0_PHA1);
    GPIOPinConfigure(GPIO_PG1_PHB1);
    //配置GPIO引脚为QEI外设
    GPIOPinTypeQEI(GPIO_PORTG_BASE,GPIO_PIN_0);
    GPIOPinTypeQEI(GPIO_PORTG_BASE,GPIO_PIN_1);
    //GPIO引脚配置为上拉
    GPIOPadConfigSet(GPIO_PORTG_BASE,GPIO_PIN_0,GPIO_STRENGTH_4MA,GPIO_PIN_TYPE_STD_WPU);
	GPIOPadConfigSet(GPIO_PORTG_BASE,GPIO_PIN_1,GPIO_STRENGTH_4MA,GPIO_PIN_TYPE_STD_WPU);
    QEIConfigure(QEI1_BASE,(QEI_CONFIG_CAPTURE_A_B      //使用AB相双边沿计数
                |QEI_CONFIG_NO_RESET	   //索引脉冲不复位位置积分器。
                |QEI_CONFIG_QUADRATURE      //在ChA和ChB上提供正交信号
                |QEI_CONFIG_NO_SWAP),       //交换ChA和ChB的通道
                0);
    QEIVelocityConfigure(QEI1_BASE,QEI_VELDIV_1,SysCtlClockGet()/50);
	QEIVelocityEnable(QEI1_BASE);
    QEIEnable(QEI1_BASE);       //使能正交编码器
    QEIIntRegister(QEI1_BASE,QEI1_IRQHandler);
    QEIIntEnable(QEI1_BASE,QEI_INTTIMER);//函数启用指定的正交编码器中断源。中断将被发送到中断控制器
    IntEnable(INT_QEI1);        //使能中断
}

